#include "SimulationManager.h"
#include "ParticleIntegrator.h"
#include "3DCamera.h"
#include "glm\glm.hpp"
#include "TwoRadGridder.h"
#include "Particle.h"
#include "Sprite.h"
#include <cstdio>
#include <cmath>
#include <ctime>
#include "Kernels.h"
#include "Helpers.h"

#define WINDOW_W 600
#define WINDOW_H 600

glm::vec3 gravity(0.0, -9.8, 0.0);

Kernel kernel;
float P0MOD = 1.0;
#define GRID_EPSILON 0.000
#define GAS_CONSTANT_K 10.0f//water
#define VISCOSITY_CONSTANT 1.0f
#define GAMMA 7.0f
glm::vec3 partmin (3, 4, 0.0),
		partmax(6, 8, 0.0);
#define GRID_MIN_PT 0 //used for grid
#define GRID_MAX_PT 10
#define MAX_PT 10 //uSED FOR CAMERA AND BOUNDS
#define MIN_PT 0
#define PARTICLE_INTERVAL 0.2
#define GRID_MASS 1.0f

#define COEFF_REST 0.0
float supportRad = 0.5f;
float kernelh = supportRad;//supportRad /1.0f;
float kernelhSq = kernelh*kernelh;
float supportRadSq = supportRad*supportRad;
unsigned int gridNum = ((MAX_PT - MIN_PT)/supportRad)/2;
float particleMass = 1.0;

#define TIME_STEP 0.005f
#define COL_DIST 0.01
float P0 = 2.447939;
typedef TwoRadGridder< ParticleS, 2> GridderT;
typedef GridderT::GridBlock<ParticleS> GridBlockT;
typedef GridBlockT::iterator<ParticleS> GridBlockIterT;
typedef std::vector<GridderT::GridItemT>::iterator GridIter;
bool doDrawForces = false;
bool doDrawTestForces = false;

TwoRadGridder< ParticleS, 2 > * gr;

std::vector< glm::vec3 > normField;
std::vector<float> pressure;
std::vector<float> density;
std::vector<float> densityTest;
std::vector<float> pressureTest;
std::vector<glm::vec3> debugForce;

std::vector<GridBlockT> nbvec;

SimulationManager sm(WINDOW_W, WINDOW_H);

ParticleCollection pc;

ParticleCollection testGridPC; // Particles at grid points.

void GetTestGridParams(
	int pNdx,
	int & gNdx,
	int * cellNdx,
	int * cellDim,
	int * gridPNdx )
{

	gNdx = gr->GetHashVal(pc[pNdx].pos);
	cellNdx[0] = gr->GetCellNdx(pc[pNdx].pos.x, 0);
	cellNdx[1] = gr->GetCellNdx(pc[pNdx].pos.y, 1);
			
	gr->GetCellDim(cellDim[0], cellDim[1]);
			
	gridPNdx[0] = gNdx + cellNdx[1];
	gridPNdx[1] = gridPNdx[0]+1;
	gridPNdx[2] = gridPNdx[0] + cellDim[0] + 1;
		gNdx + (cellDim[0]) + (cellDim[1]) + 1;
	gridPNdx[3] = gridPNdx[2] + 1;

}

void InitGridPartPos() {
	int gridDim[2];
	float cellW;
	gr->GetCellDim(gridDim[0], gridDim[1]);
	gr->GetCellWidth(cellW);

	for (int i = 0; i < gridDim[0]+1; ++i){
		for (int j = 0; j < gridDim[1]+1; ++j){

			int ndx = j*(gridDim[0]+1) + i;

			testGridPC[ndx].pos = 
				glm::vec3(
					GRID_MIN_PT + i*cellW,
					GRID_MIN_PT + j*cellW,
					0);

			testGridPC[ndx].vel = glm::vec3(0,0,0);
			testGridPC[ndx].ndx = ndx;
			testGridPC[ndx].mass = GRID_MASS;
			testGridPC[ndx].f = glm::vec3(0,0,0);
		}
	}
}

void DrawTestGridVel(){
	//
	glBegin(GL_LINES);
	unsigned int cntr = 0;

	int ndx = 0;

	for (ConstParticleIter it = testGridPC.cbegin();
		it != testGridPC.end(); ++it, ++ndx){
			DrawVector(it->pos, it->vel, 10.0f);	
	}
	
	glEnd();	

	glBegin(GL_POINTS);
	
	for (int i = 0; i < testGridPC.size(); ++i){
		glColor3f(pressureTest[i], 0, 0);
		glVertex3fv(&testGridPC[i].pos[0]);
	}
	glEnd();
}

EulerIntegratorFunctor pi;
Sprite sprite;
Bounds bounds;
bool doDrawGrid = false;
bool doShowOutline = false;
bool doShowRad = false;

float MAX_PRES, MIN_PRES;

//Triangle strip
std::vector< glm::vec2 > circleVerts;

//Mouse information
struct MouseRec mouseState;


//If the mouse moves, gives screen x, y.
void GLFWCALL MouseMoveCallback(int x, int y) {
	mouseState.UpdateMousePos(x, y);
}

//If a mouse button is used, it indicates button and state (see GLFW reference).
void GLFWCALL MouseButtonCallback(int butt, int state) {
	mouseState.butt = butt;
	mouseState.state = state;
	printf("Button: %d\n", butt);
}

void DebugFunc() {
	for (ParticleIter it = pc.begin();
		it != pc.end();
		++it){
		it->f.y += 100.0f;
	}
}

void InitCircleVerts( unsigned int numVerts) {
	float fracPi = 1.0f/numVerts;
	float twopi = 2*PI;

	for (int i = 0; i < numVerts + 2; ++i) {
		float x = std::cos(i*fracPi * twopi);
		float y = std::sin(i*fracPi * twopi);
		circleVerts.push_back(glm::vec2(x, y));
	}
}

void ToggleDrawGrid( ) {
	doDrawGrid = !doDrawGrid;
}

void ToggleDrawOutlines() {
	doShowOutline = !doShowOutline;
}



//Display
//This function will be called as fast as possible, unlike the Update func.
//Only GL display code should be put in this section.  The screen
//is automatically cleared to white in the InitFrames() func, and the 
//glfwSwapBuffers() is called by the default EndFrames() function, so you only
//have to worry about actual drawing here.
void Display() {

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(MIN_PT, MAX_PT, MIN_PT, MAX_PT, 0.01, 10.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(0,0,-1);

	glColor3f(0.0, 1.0, 0.0);
	Draw2DBox(bounds);

	glColor3f(1.0, 0.0, 0.0);
	DrawParticles(pc, pressure);

	glColor3f(1.0,1.0,0.0);
	//DrawNormals();

	if (doDrawForces) {
		DrawForces(debugForce, pc);
	}

	glColor3f(0.0, 1.0, 0.0);
	if (doDrawGrid){ 
		gr->Draw();
	}
	if (doShowRad) {
		glColor3f(0.0, 0.0, 1.0);
		DrawSupportRad(pc, supportRad, circleVerts);
	}

	if (doShowOutline) {
		DrawOutlines(pc, circleVerts);
	}

	if (doDrawTestForces){
		DrawTestGridVel();
	}

}

void ForceBounds (const Bounds & inBounds, ParticleIter & pi){
	for (int i = 0; i < 2; ++i){
		if (pi->pos[i] < inBounds.min[i]) {
			pi->vel[i] = 0;
			pi->pos[i] = inBounds.min[i];
			//pi.vel()[i] *= -1*COEFF_REST;
		}
		if (pi->pos[i] > inBounds.max[i]) {
			pi->vel[i] = 0;
			pi->pos[i] = inBounds.max[i];
			//pi.vel()[i] *= -1*COEFF_REST;
		}
	}
}

void EmitBlock (glm::vec3 & min, glm::vec3 & max, float diam){

	for (float x = min.x; x < max.x; x += diam){
		for (float y = min.y; y < max.y; y+= diam){ 
			ParticleS part;
			part.pos = glm::vec3(x, y, 0.0);
			part.vel = glm::vec3(0.0, 0.0, 0.0);
			part.mass = particleMass;
			part.ndx = pc.size();
			pc.push_back(part);
		}
	}
	
	density.resize(pc.size());
	pressure.resize(pc.size());

	normField.resize(pc.size());
	nbvec.resize(pc.size());

	printf("We now have %d particles\n", pc.size());
}


/*glm::vec3 SumFunction( ParticleIter & p,
						GridBlock & nh,
						*/


//XXX Must precompute pressure at each particle and densities
glm::vec3 ComputePressure( const glm::vec3 & pi, const glm::vec3 & pj, float rad) {
	//Compute kernel gradient.
	float r = glm::length(pi-pj);
	return  (pi - pj)*kernel.KernelSpikeyGrad( r );
}


//Particle iterator and
//density
float CalcDensity( const ParticleIter & inP,
					GridBlockT & nh)
{
	float d = 0.0f;
	//Loop through particles in block)
	for (GridBlockIterT it = nh.begin();
		it != nh.end();
		++it){
			glm::vec3 diff = inP->pos - it.GetData()->pos;
			d += it.GetData()->mass *  kernel.KernelPoly6(glm::dot(diff, diff));
	}

	return d;
}

float CalcDensity( const ParticleS & p1,
	const ParticleS & p2 ){
	
		float d = 0.0f;
		glm::vec3 diff = p1.pos - p2.pos;
		d += p1.mass *  kernel.KernelPoly6(glm::dot(diff, diff));

	return d;
}

glm::vec3 CalcNormalField( const ParticleIter & inP,
						GridBlockT &nh,
						const std::vector<float> & density){
	const glm::vec3 & inPos = inP->pos;
	glm::vec3 n;
	//Loop through cells
	for (GridBlockIterT it = nh.begin();
		it != nh.end();
		++it){
		
			float densRecpi = (1.0f/density[it.GetData()->ndx]);

			glm::vec3 diff = (inPos - it.GetData()->pos);
			glm::vec3 tmpNorm = it.GetData()->mass * densRecpi * diff * kernel.KernelPoly6Grad(glm::length(inPos - it.GetData()->pos));

			n += tmpNorm;
	}

	return n;	
}

	

glm::vec3 CalcForce( const ParticleIter & inP,
								GridBlockT & nh,
								std::vector< float > & density,
								std::vector<float> & pressure) {

	unsigned int ndx = inP->ndx;						
	const glm::vec3 & inPos = inP->pos;
	glm::vec3 force (0.0f, 0.0f, 0.0f);

	//Loop through cells
	for (GridBlockIterT gbIt = nh.begin(); 
		gbIt != nh.end();
		++gbIt){
			GridIter it = gbIt.GetData();

			unsigned int nNdx = it->ndx;
			glm::vec3 diffVel = (it->vel - inP->vel);
			glm::vec3 diffPos = ((inP->pos - it->pos));
			float len = glm::length(diffPos);
			glm::vec3 normDiffPos;
			if (len != 0){
				normDiffPos = diffPos/len;
			}
			//	glm::vec3 normDiffPos = glm::normalize(diffPos);
			//Viscosity
			float r = glm::length(diffPos);
			force = force + VISCOSITY_CONSTANT*(it->mass * (diffVel / density[nNdx])*kernel.KernelViscosityLaplac( r ) );
			//Pressure
			float weight = it->mass * (pressure[ndx] + pressure[nNdx]) / (2*(density[ndx]));
			glm::vec3 pressForce = -(weight * (normDiffPos * (kernel.KernelSpikeyGrad(r))));
			force = force + pressForce;
	}

	return force;
}

glm::vec3 CalcForce( const ParticleS & p1,
					const ParticleS & p2,
					std::vector< float > & density,
					std::vector<float> & pressure) {

	unsigned int ndx = p1.ndx;						
	const glm::vec3 & inPos = p1.pos;
	glm::vec3 force (0.0f, 0.0f, 0.0f);

	unsigned int nNdx = p2.ndx;

	glm::vec3 diffVel = (p2.vel - p1.vel);
	glm::vec3 diffPos = ((p1.pos - p2.pos));
	float len = glm::length(diffPos);
	glm::vec3 normDiffPos;
	if (len != 0){
		normDiffPos = diffPos/len;
	}
		//	glm::vec3 normDiffPos = glm::normalize(diffPos);
		//Viscosity
	float r = glm::length(diffPos);

	force = force + VISCOSITY_CONSTANT*(p1.mass * (diffVel / density[nNdx])*kernel.KernelViscosityLaplac( r ) );

		//Pressure
		float weight = p1.mass * (pressure[ndx] + pressure[nNdx]) / (2*(density[ndx]));
		glm::vec3 pressForce = -(weight * (normDiffPos * (kernel.KernelSpikeyGrad(r))));
		force = force + pressForce;


	return force;
}

class Timer {
public:
	void Start() {
		start = clock();
	}

	void Stop() {
		stop = clock();
	}

	float GetVal () {
		return ((float)stop - (float)start) / (float) CLOCKS_PER_SEC;
	}

	clock_t GetTicks() {
		return stop - start;
	}
private:
	clock_t start;
	clock_t stop;
};


void QueryNeighbors(){

	//Copy particles to grid.
	gr->Clear();
	gr->InsertData(pc);

	for (int i = 0; i < pc.size(); ++i){
		nbvec[i] = gr->GetNeighborhoodBlock(pc[i]);
	}

}

void IntegrateParticles(float ts){

	QueryNeighbors();

	MAX_PRES = std::numeric_limits<float>::min();
	MIN_PRES = std::numeric_limits<float>::max();

	//Iterate through the particles ones to compute density and pressure at each particle.
	for (ParticleIter pit = pc.begin();
		pit != pc.end();
		++pit){
		//Get the neighborhood.
			
			//GridBlockT nh = gr->GetNeighborhoodBlock(*pit);
			unsigned int ndx = pit->ndx;
			GridBlockT & nh = nbvec[ndx];


			density[ndx] = CalcDensity(pit, nh);

			//Here if gamma == 1.0, then we don't need to compute the power. It's a bit quicker.

			//GAMMA == 1.0 means we don't need a power function. it's a bit quicker.
			if (GAMMA == 1.0){
				pressure[ndx] = GAS_CONSTANT_K * ((density[ndx]/P0) - 1);//GAS_CONSTANT_K * (density[ndx] - P0);
			}
			else {
				pressure[ndx] = GAS_CONSTANT_K * (std::pow(density[ndx]/P0, GAMMA) - 1);//GAS_CONSTANT_K * (density[ndx] - P0);
			}
			
			//For normalizing it and displaying the pressure.
			if (pressure[ndx] > MAX_PRES) {
				MAX_PRES = pressure[ndx];
			}
			if (pressure[ndx] < MIN_PRES) {
				MIN_PRES = pressure[ndx];
			}

	}

	//Drawing the force directions on each particle.
	if (doDrawForces) {
		debugForce.clear();
	}
	
	for (ParticleIter pit = pc.begin();
		pit != pc.end();
		++pit){
			//
			//Add forces.
			//
	
			GridBlockT & nh = nbvec[pit->ndx];
			pit->f += CalcForce(pit, nh, density, pressure); 

			//Gravity
			//Only add gravity if it is below a certain bounds.
			if (pit->pos.y > bounds.min.y){
				pit->f.y += gravity.y*pit->mass;
			}

			//Draw the forces for  debugging.
			if (doDrawForces) {
				float len = glm::length(pit->f);
				debugForce.push_back(pit->f/len);
			}

			//Perform timestepping.
			pi.Integrate(pit, ts, density[pit->ndx]);

			//
			//Boundary conditions
			//Will make sure we don't overstep our bounds.
			ForceBounds(bounds, pit);
	}
}



void TransferVelocityToGrid() {

	int gridPNdx[4];
	int cellNdx[2];

	float invpart = 1.0f/pc.size();
	float cellW = gr->GetCellWidth();
	int cellDim[2];
	gr->GetCellDim(cellDim[0], cellDim[1]);
	for (ParticleIter pit = pc.begin();
		pit != pc.end();
		++pit) {

			glm::vec3 vel = pit->vel;
			
			//Get grid cell
			int gNdx = gr->GetHashVal(pit->pos);
			cellNdx[0] = gr->GetCellNdx(pit->pos.x, 0);
			cellNdx[1] = gr->GetCellNdx(pit->pos.y, 1);

			gridPNdx[0] = gNdx + cellNdx[1];
			gridPNdx[1] = gridPNdx[0]+1;
			gridPNdx[2] = gridPNdx[0] + cellDim[0] + 1;
				gNdx + (cellDim[0]) + (cellDim[1]) + 1;
			gridPNdx[3] = gridPNdx[2] + 1;

			// Bilin interp the velocity to the grid points
			
			float uv[2] =  {
				(pit->pos.x - testGridPC[gridPNdx[0]].pos.x) / cellW,
				(pit->pos.y - testGridPC[gridPNdx[1]].pos.y) / cellW
			};
	
			testGridPC[gridPNdx[0]].vel += (1-uv[1])*(1-uv[0])*pit->vel*invpart;
			testGridPC[gridPNdx[1]].vel += (1-uv[1])*(  uv[0])*pit->vel*invpart;
			testGridPC[gridPNdx[2]].vel += (  uv[1])*(1-uv[0])*pit->vel*invpart;
			testGridPC[gridPNdx[3]].vel += (  uv[1])*(  uv[0])*pit->vel*invpart;
	}
}

void InterPartGridPos(){
	int gridPNdx[4];
	int cellNdx[2];

	float invpart = 1.0f/pc.size();
	float cellW = gr->GetCellWidth();
	int cellDim[2];
	gr->GetCellDim(cellDim[0], cellDim[1]);
	int pndx = 0;
	int gNdx;

	for (ParticleIter pit = pc.begin();
		pit != pc.end();
		++pit, ++pndx) {
			//Get grid cell
			GetTestGridParams(pndx, gNdx, cellNdx, cellDim, gridPNdx);

			// Bilin interp the velocity to the grid points
			float uv[2] =  {
				(pit->pos.x - testGridPC[gridPNdx[0]].pos.x) / cellW,
				(pit->pos.y - testGridPC[gridPNdx[1]].pos.y) / cellW
			};
	
			pit->pos = 
				(1-uv[1])*(testGridPC[gridPNdx[0]].pos*(1-uv[0]) +
				testGridPC[gridPNdx[1]].pos*(uv[0])) +
				(uv[1])*(testGridPC[gridPNdx[2]].pos*(1-uv[0]) +
				testGridPC[gridPNdx[3]].pos*(uv[0]));
			pit->vel =
				(1-uv[1])*(testGridPC[gridPNdx[0]].vel*(1-uv[0]) +
				testGridPC[gridPNdx[1]].vel*(uv[0])) +
				(uv[1])*(testGridPC[gridPNdx[2]].vel*(1-uv[0]) +
				testGridPC[gridPNdx[3]].vel*(uv[0]));

	}
}

void IntegrateParticlesTest(float ts){

	InitGridPartPos();

	for (int i = 0; i < testGridPC.size(); ++i){
		densityTest[i] = 0;
		pressureTest[i] = 0;
		testGridPC[i].f = glm::vec3(0,0,0);
	}

	MAX_PRES = std::numeric_limits<float>::min();
	MIN_PRES = std::numeric_limits<float>::max();

	TransferVelocityToGrid();

	int gNdx;
	int cellNdx[2];		
	int cellDim[2];
	int gridPNdx[4];
			

	//Iterate through the particles ones to compute density and pressure at each particle.
	int pNdx = 0;
	for (ParticleIter pit = pc.begin();
		pit != pc.end();
		++pit, ++pNdx){

			//	
			//Get grid cell
			GetTestGridParams( pNdx, gNdx, cellNdx, cellDim, gridPNdx);
			

			// Map to each grid cell.
			for (int i = 0; i < 4; ++i){
				int ndx = gridPNdx[i];

				densityTest[ndx] += CalcDensity(testGridPC[ndx], *pit);
				
			}
	}

	// Do pressure
	for (int i = 0; i < testGridPC.size(); ++i){
			int ndx = i;
			//Here if gamma == 1.0, then we don't need to compute the power. It's a bit quicker.

			//GAMMA == 1.0 means we don't need a power function. it's a bit quicker.
			if (GAMMA == 1.0){
				pressureTest[ndx] = GAS_CONSTANT_K * ((densityTest[ndx]/P0) - 1);//GAS_CONSTANT_K * (density[ndx] - P0);
			}
			else {
				pressureTest[ndx] = GAS_CONSTANT_K * (std::pow(densityTest[ndx]/P0, GAMMA) - 1);//GAS_CONSTANT_K * (density[ndx] - P0);
			}
	}

	//pNdx = 0;
	//for (ParticleIter pit = pc.begin();
	//	pit != pc.end();
	//	++pit, ++pNdx){
	//		//	
	//		//Get grid cell
	//		GetTestGridParams( pNdx, gNdx, cellNdx, cellDim, gridPNdx);
	//	
	//		//
	//		//Add forces.
	//		//
	//		for (int i = 0; i < 4; ++i){
	//			glm::vec3 f = CalcForce(testGridPC[gridPNdx[i]],
	//					*pit,
	//					density,
	//					pressure );
	//			testGridPC[gridPNdx[i]].f += f;
	//				
	//		}
	//}
	debugForce.clear();
	for (ParticleIter pit = testGridPC.begin();
		pit != testGridPC.end();
		++pit, ++pNdx){
		pit->f.y += gravity.y*pit->mass;
			//Draw the forces for  debugging.
			if (doDrawTestForces) {
				debugForce.push_back(pit->f);
			}

		pi.Integrate(pit, ts, densityTest[pit-testGridPC.begin()]);

		ForceBounds(bounds, pit);
	}

	// Interpolate back.
	InterPartGridPos();

}


void CalcConstPress() {
	float p0 = 0;

	for (ParticleIter pit = pc.begin();
		pit != pc.end();
		++pit){
		
			GridBlockT nh = gr->GetNeighborhoodBlock(*pit);

			unsigned int ndx = pit->ndx;

			float dens = CalcDensity(pit, nh);

			p0 += dens / pc.size();
	}


	printf("Average dnesity: %f\n", p0);

	P0 = p0*P0MOD;

	//P0 = 1000.0;
	printf("Emitted %d particles\n", pc.size());
}

void DoShowRad() {
	doShowRad = !doShowRad;
}

void DoEmit() {
	static bool hasCalcConst = false;

	EmitBlock(partmin, partmax, PARTICLE_INTERVAL);

	//Recalculate P0
	
	//Copy particles to grid.
	gr->Clear();
	gr->InsertData(pc);
	if (!hasCalcConst){
		//Calculates the constant pressure from the uniform grid.
		CalcConstPress();
		hasCalcConst = true;
	}
	
}

//Update
//This function will be called every timestep (ts).  The timestep can be
//set via the SetTimeStep method in SimulationManager. Put all non-drawing code
//here.
void Update(float ts) {

	IntegrateParticles( ts );

}

void UpdateTest(float ts) {
	//IntegrateParticles(ts);
	IntegrateParticlesTest( ts );

}

void Pause() {
	sm.Pause();
}

void DoDrawForces() {
	doDrawForces = !doDrawForces;
}

void DoDrawTestForces() {
	doDrawTestForces = !doDrawTestForces;
}

void LoadResources() {
	SpriteResource res;
	std::string filename = "ball.tga";
	if (!res.Load(filename)) {
		printf("Could not load file %s\n", filename.c_str());	
	}

	sprite.SetResource(res);

	//Key callbacks
	sm.AddKeyCallback('P', Pause);
	sm.AddKeyCallback('G', ToggleDrawGrid);
	sm.AddKeyCallback('D', DebugFunc);
	sm.AddKeyCallback('O', ToggleDrawOutlines);
	sm.AddKeyCallback('A', DoEmit);
	sm.AddKeyCallback('R', DoShowRad);
	sm.AddKeyCallback('F', DoDrawForces);
	sm.AddKeyCallback('T', DoDrawTestForces);

}

void FillGridParticles() {
	int gridDim[2];

	gr->GetCellDim(gridDim[0], gridDim[1]);

	int numParts = (gridDim[0]+1)*(gridDim[1]+1);
	testGridPC.resize(numParts);
	densityTest.resize(testGridPC.size());
	pressureTest.resize(testGridPC.size());
	
	InitGridPartPos();
}

void Init() {

	//Sets the kernel's support radius
	kernel.SetKernelH(supportRad);

	//Min/max for our grid.
	glm::vec2 minPt = glm::vec2(GRID_MIN_PT, GRID_MIN_PT);
	glm::vec2 maxPt = glm::vec2(GRID_MAX_PT, GRID_MAX_PT);

	gr = new TwoRadGridder<ParticleS, 2> ( &minPt[0], &maxPt[0], supportRad );

	// XXX TEST
	FillGridParticles();

	//
	//Set Simulation Manager callbacks.
	sm.SetCallbackLoadResources(LoadResources);
	sm.SetCallbackDisplay(Display);
	sm.SetCallbackUpdate(UpdateTest);


	//
	//Time step
	sm.SetTimeStep(TIME_STEP);


	//Simulation boundary
	bounds.min = glm::vec3(MIN_PT, MIN_PT, 0);
	bounds.max = glm::vec3(MAX_PT, MAX_PT, 0);

	InitCircleVerts(20);

	glEnable(GL_DEPTH_TEST);
}

int main (int argc, char ** argv ){

	//Init
	Init();

	//and...

	//Start simulation.
	sm.Run();
}